#include "includes.h" 

//电机PID
//float M_kp=1.88;  //  0.260    //1.88    自整定
float M_kp=6.6;  //3.5 
float M_ki=0.04;  //0.091   0.13
float M_kd=2.13; 

signed int setspeed,speednow;
  
signed int pre_error;       //速度PID 前一次的速度误差值ideal_speed- pulse_count 
signed int pre_error2;      //速度PID 前2次的速度误差值ideal_speed- pulse_count 
signed int pre_d_error;     //速度PID 前一次的速度误差之差d_error-pre_d_error  
signed int pk;              //速度PID值   

                            //舵机PD定义
float S_Kp;                 //比例常数
float S_Kd; 
float servo_P;     //微分常数
float S_E0=0;               //当前误差
float S_E1=0;               //上次误差
float S_E2=0;              
float S_Error0=0;
float S_Error1=0;         
int iIncpid;

unsigned int change_tempp,change_temps;
int stop_flag=0;
extern int red_pflag,z_flag,Averagevalue;
extern unsigned int ss;
extern float spp,sdd;
extern int coordinate;

/*
void value_change(void) 
{
  if(speednow<=20)
  {
   change_tempp=ReadATD(5);
   change_temps=ReadATD(6);
  }
}   */




  void Steer_PD(float sp,float sd,int ch1)    //位置式PD  
{
    S_Kp=sp;
    S_Kd=sd;
    S_E0=ch1;
    S_Error0=S_E0-S_E1;  
    
    if(S_E0*S_Error0>=0)
    servo_P+=S_Kp*S_Error0*S_E0;
     else if(S_E0*S_Error0<0)
     servo_P+=S_Kp*(S_Error0/S_E0)*100;
   if(servo_P>=15)
     servo_P=15;
  if(servo_P<=8)
     servo_P=8;
  iIncpid=(int)(4200-(servo_P*S_E0)-(S_Kd*S_Error0));
    S_E1=S_E0; 
    
    if (iIncpid<3300)
     iIncpid=3300;
    if (iIncpid>5200) 
     iIncpid=5200;
    angle=iIncpid;                                                                                                                                         
   
    
} 



/* int Pcrtl(float ch1)
{
 E0=ch1;
 Error0=E0-E1;
   if(E0*Error0>=0)
 servo_P+=Np*Error0*E0;
 else if(E0*Error0<0)k
   servo_P+=Np*(Error0/E0)*100;
 if(servo_P>=7)
   servo_P=7;
  if(servo_P<=0.5)
   servo_P=0.5;
 Error1=E1-E2;
 pwm_data=(int)((servo_P*E0)+(servo_D*(float)(Error1)));
 if(pwm_data>73)/////////////////////参数有待调整//////////////////
   pwm_data=73;
  if(pwm_data<-73)
   pwm_data=-73;
E2=E1;
E1=E0;
return pwm_data;
} */

 
//电机PID  
void Motor_PID(int in1)   
{  
   signed int m_error,d_error,dd_error,Bang;  
   static int start_flag=0; 
       
   if(start_flag<250)
    start_flag++;  
   
   setspeed=in1;
   m_error = setspeed - speednow; 
   d_error = m_error - pre_error;  
   dd_error = d_error - pre_d_error;  
   
          
   pre_error = m_error; //存储当前偏差  
   pre_d_error = d_error;  
   
   
   if(abs(m_error)<5) 
     m_error=0;
   
 if(m_error<40&&m_error>-30)
   Bang=1;
 else
    Bang=0;
 
   if(Bang==1) 
   {
    
   pk += (signed int)(M_kp * d_error +M_ki*m_error+M_kd * dd_error);  
          
   if(pk >= 900)   
   {  
     pk = 900;  
   }  
   else if(pk <= 0)   
   {  
     pk = 0;  
   }  
   
   } 
   
   else 
  {
     if(m_error>=40)
       pk = 900; 
     else if(m_error<=-30)
     pk=0;

   }
   if(start_flag>=222) 
   {
     speed_go=pk;
     start_flag=262;  
   }
   if(Red_stop1==1||Red_stop2==1)   //检测跑出赛道停车保护
   {                
      red_pflag=1;        //用来控制蓝牙发送
      PWME_PWME3=0;
      PWME_PWME5=0;
   }     /**/
      
/*  
   if(start_flag>=1600) 
   {
     if(Red_stop1==1||Red_stop2==1)   //检测跑出赛道停车保护
     {                
        stop_flag++;
        red_pflag=1;        //用来控制蓝牙发送
        start_flag=2000; 
      }
    }
   
    if(stop_flag>=1&&z_flag>=20&&abs(Averagevalue)<10)        
    {
        if(speednow>10) 
        { 
          PWME_PWME3=0;
          speed_back=468;
          Steer_PD(spp,sdd,coordinate);
        } 
        else 
        {
          Steer_PD(spp,sdd,coordinate);
          PWME_PWME5=0;
        } 
     }
    */
}


  
/*  
void Motor_PID(int in1)         //电机PID 
{  
   signed int m_error;
   static int start_flag=0,finish_flag=0; 
  
   if(start_flag<1600)
   start_flag++;  
   
   setspeed=in1;
   if(a_b(setspeed,speednow)<3)
   speednow=setspeed;
          
   m_error = setspeed - speednow;  
        
   pre_error2=pre_error;  
   pre_error = m_error; //存储当前偏差   
          
   // pk += (signed int)(M_kp * d_error + M_ki * m_error + M_kd * dd_error); 
   pk += (signed int)(M_kp *(2.45* m_error + 3.5 * pre_error+1.25*pre_error2)); 
          
   if(pk >= 800)   
   {    
     pk = 800;  
   }       
   else if(pk <= 0)   
   {  
     pk = 0;  
   }  
        
   speed_go=pk; 
   
 
   if(start_flag>=1600) 
   {
     if(Red_stop1==1||Red_stop2==1)   //检测跑出赛道停车保护
     {                
        stop_flag++;
        red_pflag=1;        //用来控制蓝牙发送
        start_flag=2000; 
      }
    }
   
    if(stop_flag>=1&&z_flag>=20&&abs(Averagevalue)<10)        //    &&abs((int)Averagevalue)<15
     {
       if(finish_flag==0) 
       {  
         if(speednow>10) 
         { 
           PWME_PWME3=0;
           speed_back=668;
           Steer_PD(spp,sdd,coordinate);
         } 
         else 
         {
           Steer_PD(spp,sdd,coordinate);
           finish_flag=1;
           PWME_PWME5=0;
         } 
       }
     }
}     */
 





    
  
